// ROS includes
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>

#include "ft_nodes/fault_injector.h"

// c++ includes
#include <cstdlib>
#include <cstdio>
#include <fstream>

enum E_FaultType {
    
    EFT_None = 0,
    EFT_LeftActuator,
    EFT_RightActuator,
    EFT_BothActuators,
    EFT_Odometry
    
} g_eCurrentFault;

ros::Publisher      g_oCmdvelPub;
ros::Publisher      g_oOdometryPub;
std::string         g_strCurrentFault;

nav_msgs::Odometry  g_oLastOdomReceived;

void OdometryCallback( const nav_msgs::Odometry::ConstPtr& msg ) {
    
    if ( g_eCurrentFault != EFT_Odometry ) {
        
        g_oLastOdomReceived = *(msg.get());
    }
    g_oOdometryPub.publish( g_oLastOdomReceived );
}

void CmdVelCallback( const geometry_msgs::Twist::ConstPtr& msg ) {
    
    geometry_msgs::Twist faulty_msg = *(msg.get());
            
    switch( g_eCurrentFault ) {
        
        case EFT_BothActuators:
            faulty_msg.angular.x =
            faulty_msg.angular.y =
            faulty_msg.angular.z =
            faulty_msg.linear.x  =
            faulty_msg.linear.y  =
            faulty_msg.linear.z  = 0.0;
            break;

        case EFT_LeftActuator:
            faulty_msg.angular.z  = faulty_msg.linear.x;
            faulty_msg.linear.x /= 2.0;
            break;

        case EFT_RightActuator:
            faulty_msg.angular.z  = -faulty_msg.linear.x;
            faulty_msg.linear.x /= 2.0;
            break;
    }
    
    g_oCmdvelPub.publish( faulty_msg );
}

bool ConfigCallback( ft_nodes::fault_injectorRequest& request, ft_nodes::fault_injectorResponse& response ) {

    if ( request.fault == "cancel" ) {
        
        if ( g_strCurrentFault.empty() ) {
            
            response.return_message = "there isn't anything to cancel!";
        }
        else {
            
            response.return_message = g_strCurrentFault + " fault injection has been cancelled.";
            g_eCurrentFault = EFT_None;
            g_strCurrentFault.clear();
        }
    }
    else if ( request.fault == "left_actuator" ) {
        
        if ( !g_strCurrentFault.empty() ) {
            
            response.return_message = g_strCurrentFault + " fault injection has been cancelled. | ";
        }
        response.return_message += ( request.fault + " fault injection activated." );
        g_eCurrentFault   = EFT_LeftActuator;
        g_strCurrentFault = request.fault;
    }
    else if ( request.fault == "right_actuator" ) {
        
        if ( !g_strCurrentFault.empty() ) {
            
            response.return_message = g_strCurrentFault + " fault injection has been cancelled. | ";
        }
        response.return_message += ( request.fault + " fault injection activated." );
        g_eCurrentFault   = EFT_RightActuator;
        g_strCurrentFault = request.fault;
    }
    else if ( request.fault == "both_actuators" ) {
        
        if ( !g_strCurrentFault.empty() ) {
            
            response.return_message = g_strCurrentFault + " fault injection has been cancelled. | ";
        }
        response.return_message += ( request.fault + " fault injection activated." );
        g_eCurrentFault   = EFT_BothActuators;
        g_strCurrentFault = request.fault;
    }
    else if ( request.fault == "odometry" ) {
        
        if ( !g_strCurrentFault.empty() ) {
            
            response.return_message = g_strCurrentFault + " fault injection has been cancelled. | ";
        }
        response.return_message += ( request.fault + " fault injection activated." );
        g_eCurrentFault   = EFT_Odometry;
        g_strCurrentFault = request.fault;
    }
    else {
        
        response.return_message = "Unrecognized instruction";
    }
}


int main(int argc, char **argv) {
    
    ros::init(argc, argv, "fault_injector");
    ros::NodeHandle n;
    
    ros::Subscriber    subD    = n.subscribe( "/fault_injector/cmd_vel", 10, CmdVelCallback   );
    ros::Subscriber    subO    = n.subscribe( "/odom"   , 10, OdometryCallback );
    
    ros::ServiceServer service = n.advertiseService( "/fault_injector", ConfigCallback    );
    
    g_oCmdvelPub   = n.advertise< geometry_msgs::Twist >( "/cmd_vel", 10 );
    g_oOdometryPub = n.advertise< nav_msgs::Odometry   >( "/fault_injector/odom"   , 10 );
    
    ros::spin();
    
    return 0;	
}